package simulator;

import java.util.ArrayList;

/**
 * Environment consists of all objects in the world, Along with their placements
 * and the placement of the robot. All interactions in world occur based on this
 * data
 * 
 * @author kroo
 */

public class Environment {
	// list of all objects in the environment
	private ArrayList<PhysicalObject> objects = new ArrayList<PhysicalObject>();

	// access to the environment (see sharedInstance())
	protected static Environment sharedEnvironment = new Environment();

	// current location & orientation of the robot
	protected double robotX, robotY, robotAngle;

	public void setStart(double x, double y, double o) {
		robotX = x;
		robotY = y;
		robotAngle = o;
	}

	/**
	 * Default Constructor -- you should probably access the shared instance
	 * instead.
	 * 
	 * @see sharedInstance
	 */
	public Environment() {
	}

	/**
	 * Provides static access to the current environment object. Note: this is
	 * not safe in situations when more than one simulation is running in the
	 * same JVM.
	 * 
	 * @return the current environment object
	 */
	public static Environment sharedInstance() {
		return sharedEnvironment;
	}

	/**
	 * Add an object to the environment.
	 * 
	 * @param o
	 *            the object to be added
	 * @see PhysicalObject
	 */
	public void add(PhysicalObject o) {
		objects.add(o);
	}

	/**
	 * Get the i'th object in the environment (convenience method)
	 * 
	 * @return the i'th object in the environment
	 * @param i
	 *            index of the object
	 */
	public PhysicalObject get(int i) {
		return objects.get(i);
	}

	/**
	 * Retreives the array of PhysicalObjects in the environment
	 * 
	 * @return every object in the environment
	 */
	public ArrayList<PhysicalObject> getObjects() {
		return objects;
	}

	/**
	 * Removes an object from the environment
	 * 
	 * @param o
	 *            the object to be removed
	 */
	public void remove(PhysicalObject o) {
		objects.remove(o);
	}

	// These are local objects for ranged sensors like sonar
	/**
	 * NOTE This method will return all known local objects, which simulated
	 * sensors will need to filter by the actual reach of the hardware
	 */
	public ArrayList<PhysicalObject> getLocalObjects(double x, double y,
			double radius) {

		/*
		 * ArrayList<PhysicalObject> locals = new ArrayList<PhysicalObject>();
		 * for(int i=0; i<objects.size(); i++) { PhysicalObject local =
		 * objects.get(i);
		 * 
		 * if(Math.sqrt( (x - local.getX()) * (x - local.getX()) + (y -
		 * local.getY()) * (y - local.getY()) ) <= radius){
		 * 
		 * locals.add(local); }
		 * 
		 * }
		 */
		return objects;
	}

	/**
	 * Updates the bot's location and orientation (called from Simulator)
	 * 
	 * @param x
	 *            the x component of the bots location
	 * @param y
	 *            the y component of the bots location
	 * @param angle
	 *            the bots 2d orientation (in radians)
	 * @see Simulator
	 */
	public void updateLocation(double x, double y, double angle) {
		robotX = x;
		robotY = y;
		robotAngle = angle;
	}

	/**
	 * Returns the robots current x location
	 * 
	 * @return the robot's x component of its location
	 */
	public double getRobotX() {
		return robotX;
	}

	/**
	 * Returns the robots current y location
	 * 
	 * @return the robot's y component of its location
	 */
	public double getRobotY() {
		return robotY;
	}

	/**
	 * Returns the robots current 2D orientation
	 * 
	 * @return the robot's orientation (right = 0, ccw, units are radians)
	 */
	public double getRobotAngle() {
		return robotAngle;
	}
}